#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <vector>
#include <math.h>
#include <fstream>
#include <iomanip>
#include <opencv2/calib3d.hpp>
using namespace cv;
#include <iostream>
using namespace std;


void Draw_FilledDot(Mat img, Point center) {    //瞄准点绘制
    int thickness = -1;
    int lineType = 8;
    circle(img,
           center,
           8,
           Scalar(0, 0, 255),
           thickness,
           lineType);
}


int Targeting_algorithms() {
    VideoCapture video;
    video.open("../test111.avi");
    if (!video.isOpened())  //打不开，报错
    {
        cout << "could not open the Video!" << endl;
        system("pause");
        return -1;
    }

    const char* windowsName = "?é×???·¨";
    int k = -1;
    while (true)
    {
        Mat img,grayImage,gray_binary,redChannel;
        Mat element = getStructuringElement(MORPH_RECT, Size(6, 6));  //定义开运算的核
        bool ok = video.read(img);
        Mat xianshi = img.clone();
        if (!ok)
            break;
        cvtColor(img, grayImage, COLOR_BGR2GRAY);             //图像灰度处理
        //blur(grayImage, grayImage, Size(3, 3), Point(-1, -1));
        threshold(grayImage, gray_binary, 125, 255, THRESH_BINARY);    //g
        morphologyEx(gray_binary, gray_binary, MORPH_OPEN,element);     //???????¨?????????ò????
        Mat midImg1 = grayImage.clone();

        Mat midImg3 = Mat::zeros(gray_binary.rows, gray_binary.cols, CV_8UC3);
        vector<vector<Point>> contours;
        vector<Vec4i> hierarchy;
        ::findContours(gray_binary, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE); //?°??????
        /*int index = 0;
        for (; index >= 0; index = hierarchy[index][0])
        {
            Scalar color(255, 255, 255);
            drawContours(midImg3, contours, index, color, NULL, 8, hierarchy);
        }*/
        Mat midImg4 = midImg3.clone();
        Point point1, point2,point_wrong,center_point;

        int j = -1;
        vector<Point3f> imgpoint;
        for (int i = 0; i < contours.size(); i++) {

            vector<Point> points = contours[i];//????????
            RotatedRect box = minAreaRect(Mat(points));//?????¨??2D???????°??×?????????°ü?§????
            Point2f vertex[4];
            box.points(vertex);
            //??????×?????????°ü?§????
            double width, lenth;

            width = sqrt(pow((vertex[0].x - vertex[1].x), 2) + pow((vertex[0].y - vertex[1].y), 2));
            lenth = sqrt(pow((vertex[1].x - vertex[2].x), 2) + pow((vertex[1].y - vertex[2].y), 2));

            if (width >= 4 * lenth || lenth >= 4 * width) {
                j++;
                ::line(xianshi, vertex[0], vertex[1], Scalar(0, 255, 0), 1, LINE_AA);
                ::line(xianshi, vertex[1], vertex[2], Scalar(0, 255, 0), 1, LINE_AA);
                ::line(xianshi, vertex[2], vertex[3], Scalar(0, 255, 0), 1, LINE_AA);
                ::line(xianshi, vertex[3], vertex[0], Scalar(0, 255, 0), 1, LINE_AA);
                //??????????????????±ê
                Point s1, l, r, u, d;
                s1.x = (vertex[0].x + vertex[2].x) / 2.0;   //s1??????????????????
                s1.y = (vertex[0].y + vertex[2].y) / 2.0;
                l.x = s1.x - 7;
                l.y = s1.y;

                r.x = s1.x + 7;
                r.y = s1.y;

                u.x = s1.x;
                u.y = s1.y - 7;

                d.x = s1.x;
                d.y = s1.y + 7;
                ::line(xianshi, l, r, Scalar(0, 0, 255), 1, LINE_AA);   //??·??ò?????????á??
                ::line(xianshi, u, d, Scalar(0, 0, 255), 1, LINE_AA);   //??·??ò?????????ú??

               // double point_aim[4]={};
                if (j == 0)point1 = s1;
                //if (point1.x == 0 && point1.y == 0)continue;
                imgpoint.push_back(Point3f(vertex[0]));
                imgpoint.push_back(Point3f(vertex[3]));
                if (j == 1) {
                    point2 = s1;

                    ::line(xianshi, point1, point2, Scalar(0, 255, 0), 1,
                           LINE_AA);    //??????????????x??y??·????????é×???
                    center_point.x = (point1.x + point2.x) / 2;
                    center_point.y = (point1.y + point2.y) / 2;
                    ::circle(xianshi, center_point, 3, Scalar(0, 0, 255), -1, LINE_AA);
                    imgpoint.push_back(Point3f(center_point));
                    Mat(imgpoint).convertTo(imgpoint,CV_32F);
                   // imgpoint.push_back(Point3f(vertex[1]));
                  //  imgpoint.push_back(Point3f(vertex[2]));
                }

            }
        }
        vector<Point3f> worldpoint;

        worldpoint.push_back(Point3f(115,30,0));
        worldpoint.push_back(Point3f(115,-30,0));
        worldpoint.push_back(Point3f(-115,30,0));
        worldpoint.push_back(Point3f(-115,-30,0));
        worldpoint.push_back(Point3f(0,0,0));
        Mat(worldpoint).convertTo(worldpoint,CV_32F);
        double camera[9] ={1.6041191539594568e+03, 0., 6.3983687194220943e+02, 0.,
                             1.6047833493341714e+03, 5.1222951297937527e+02, 0., 0., 1. };
        double distcoeffs[5] ={-6.4910709385278609e-01, 8.6914328787426987e-01,
                                 5.1733428362687644e-03, -4.1111054148847371e-03, 0. };
    Mat camera_mat = Mat(3,3,CV_64FC1,camera);
    Mat dist_mat = Mat(5,1,CV_64FC1,distcoeffs);
        vector <double> mat1,mat2;
        cout<<imgpoint<<endl;
     //   solvePnP(worldpoint, imgpoint, camera_mat, dist_mat, mat1, mat2);
        //cout<<mat1<<endl;
        ::imshow(windowsName, xianshi);
        ::imshow("????", img);
        //::imshow("??????", gray_binary);
        k = waitKey(33);                //30fps
        if (k == 27) break;    //°???Esc??????·?
    }
}

int main()
{

    Targeting_algorithms();

}